﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;
using System.Diagnostics;

namespace Drifting.MotionPlanning.Systems
{
    public class BiPedalAB : BaseSystem
    {
        double a;
        double b;
        double m;
        double mH;
        double g;
        public double phi;
        double[,] S;

        public double L_B
        {
            get { return a + b; }
        }
        public double L_A
        {
            get { return a + b; }
        }

        public BiPedalAB()
            : base("../../../MotionPlanning/Resources/Drift12D.kdl")
        {
            // phi = 3.15 
            // q[0] = 350, q[1] = 320
            
            t_delta = 0.01;
            //  model parameters
            g = 9.80665;//m/s/s
            m = 5;      //kg
            mH = 10;    //kg
            a = 0.5;    //m
            b = 0.5;    //m
            phi = 15 / 180.0 * Math.PI;    

            this.dof_ctrl = 3;
            this.dof_alias = 4;
            this.dof_state = 7;
            
            this.max_alias = new double[4];
            max_alias[0] = (360 + 55) / 180.0 * Math.PI;
            max_alias[1] = (360 + 55) / 180.0 * Math.PI;
            max_alias[2] = 5;
            max_alias[3] = 5;

            this.min_alias = new double[4];
            min_alias[0] = (360 - 55) / 180.0 * Math.PI;
            min_alias[1] = (360 - 55) / 180.0 * Math.PI;
            min_alias[2] = -5;
            min_alias[3] = -5;

            this.max_ctrl = new double[3];
            max_ctrl[0] = 0;
            max_ctrl[1] = 3;
            max_ctrl[2] = 0;

            this.min_ctrl = new double[3];
            min_ctrl[0] = 0;
            min_ctrl[1] = -3;
            min_ctrl[2] = 0;

            max_Dt = 0.3;
            min_Dt = 0.05;

            S = new double[2, 3];
            S[0, 0] = 1;
            S[0, 1] = -1;
            S[0, 2] = 0;
            S[1, 0] = 0;
            S[1, 1] = 1;
            S[1, 2] = 1;

        }

        private void translate_AB_to_NS(double[] q_BAF, ref double[] q_alias)
        {
            if (q_BAF[4] >= 0)
            {
                // B is support
                q_alias[0] = q_BAF[1];
                q_alias[1] = q_BAF[0];
                q_alias[2] = q_BAF[3];
                q_alias[3] = q_BAF[2];
                q_alias[4] = 1;
            }
            else
            {
                // A is support
                q_alias[0] = q_BAF[0];
                q_alias[1] = q_BAF[1];
                q_alias[2] = q_BAF[2];
                q_alias[3] = q_BAF[3];
                q_alias[4] = -1;
            }
            q_alias[5] = q_BAF[5];
            q_alias[6] = q_BAF[6];
        }

        private void translate_NS_to_AB(double[] q_alias, ref double[] q_BAF)
        {
            if (q_alias[4] >= 0)
            {
                // B is support
                q_BAF[0] = q_alias[1];
                q_BAF[1] = q_alias[0];
                q_BAF[2] = q_alias[3];
                q_BAF[3] = q_alias[2];
                q_BAF[4] = 1;
            }
            else
            {
                 // A is support
                 q_BAF[0] = q_alias[0];
                 q_BAF[1] = q_alias[1];
                 q_BAF[2] = q_alias[2];
                 q_BAF[3] = q_alias[3];
                 q_BAF[4] = -1;
            }
            q_BAF[5] = q_alias[5];
            q_BAF[6] = q_alias[6];
        }

        override public void SampleState(Random rand, ref double[] alias)
        {
            if (alias == null)
                alias = new double[dof_alias];

            for (int i = 0; i < dof_alias; ++i)
                alias[i] = min_alias[i] + rand.NextDouble() * (max_alias[i] - min_alias[i]);
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double u_ns = ctrl[0];
            double u_H = ctrl[1];
            double u_s = ctrl[2];

            double theta_ns = q[0];
            double theta_s = q[1];
            double dTheta_ns = q[2];
            double dTheta_s = q[3];
            double l = a + b;

            double[,] M = new double[2, 2];
            M[0, 0] = m * b * b;
            M[0, 1] = -m * l * b * Math.Cos(theta_s - theta_ns);
            M[1, 0] = M[0, 1];
            M[1, 1] = mH * l * l + m * (l * l + a * a);

            double[,] N = new double[2, 2];
            N[0, 0] = 0;
            N[0, 1] = m * l * b * dTheta_s * Math.Sin(theta_s - theta_ns);
            N[1, 0] = -m * l * b * dTheta_ns * Math.Sin(theta_s - theta_ns);
            N[1, 1] = 0;

            double[,] G = new double[2, 1];
            G[0, 0] = m * b * Math.Sin(theta_ns) * g;
            G[1, 0] = -(mH * l + m * a + m * l) * Math.Sin(theta_s) * g;

            double[,] M_inv = new double[2, 2];
            if (MathAssistant.Matrix_Inverse_2x2(M, ref M_inv) == false)
                throw new Exception("inverse failure");

            //[x3 x4] = M^{-1} * ( S U - N dTheta - G    )

            double UGN_0 = -(N[0, 0] * dTheta_ns + N[0, 1] * dTheta_s) - G[0, 0] + S[0, 0] * u_ns + S[0, 1] * u_H + S[0, 2] * u_s;
            double UGN_1 = -(N[1, 0] * dTheta_ns + N[1, 1] * dTheta_s) - G[1, 0] + S[1, 0] * u_ns + S[1, 1] * u_H + S[1, 2] * u_s;


            //dTheta_ns;
            //dTheta_s;
            double ddTheta_ns = M_inv[0, 0] * UGN_0 + M_inv[0, 1] * UGN_1;
            double ddTheta_s = M_inv[1, 0] * UGN_0 + M_inv[1, 1] * UGN_1;

            qd[0] = dTheta_ns;
            qd[1] = dTheta_s;
            qd[2] = ddTheta_ns;
            qd[3] = ddTheta_s;
        }

        public void IntegrationAlias(MemoryPad pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            Qd(q, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k1);

            MathAssistant.vector_vlinear(pad.k1, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k2);

            MathAssistant.vector_vlinear(pad.k2, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k3);

            MathAssistant.vector_vlinear(pad.k3, 1, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k4);

            MathAssistant.vector_vlinear(pad.k1, 1.0 / 6.0, q, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k2, 1.0 / 3.0, pad.Qd_out, ref pad.Qd_in);
            MathAssistant.vector_vlinear(pad.k3, 1.0 / 3.0, pad.Qd_in, ref pad.Qd_out);

            MathAssistant.vector_vlinear(pad.k4, 1.0 / 6.0, pad.Qd_out, ref q_next);

            double T_unsupport_minus = q_next[0];
            double T_support_minus = q_next[1];
            double dT_unsupport_minus = q_next[2];
            double dT_support_minus = q_next[3];

            double T_alpha = T_unsupport_minus - T_support_minus;
            double shouldBeZero = T_unsupport_minus + T_support_minus - Math.PI - Math.PI - Math.PI - Math.PI + phi + phi;

            double l = a + b;
            double cos_Alpha = Math.Cos(T_alpha);
            double d_Alpha = dT_unsupport_minus - dT_support_minus;
            double cos_support = Math.Cos(T_support_minus);
            double sin_support = Math.Sin(T_support_minus);
            double cos_unsupport = Math.Cos(T_unsupport_minus);
            double sin_unsupport = Math.Sin(T_unsupport_minus);

            double dx_step = -l * sin_support + l * sin_unsupport;
            double dy_step = l * cos_support - l * cos_unsupport;

            double aoa = Math.Atan2(dy_step, dx_step) / Math.PI * 180;

            if (
                T_unsupport_minus > Math.PI * 2 &&
                dT_unsupport_minus < 0 &&
                aoa < 0
                )
            {
                double[,] Q_minus = new double[2, 2];
                Q_minus[0, 0] = -m * a * b;
                Q_minus[0, 1] = -m * a * b + (mH * l * l + 2 * m * a * l) * cos_Alpha;
                Q_minus[1, 0] = 0;
                Q_minus[1, 1] = -m * a * b;

                double[,] Q_plus = new double[2, 2];
                Q_plus[0, 0] = m * b * (b - l * cos_Alpha);
                Q_plus[0, 1] = m * l * (l - b * cos_Alpha) + m * a * a + mH * l * l;
                Q_plus[1, 0] = m * b * b;
                Q_plus[1, 1] = -m * b * l * cos_Alpha;

                double[,] Q_plus_inv = new double[2, 2];
                if (MathAssistant.Matrix_Inverse_2x2(Q_plus, ref Q_plus_inv) == false)
                    throw new Exception("inverse failure");

                double[,] H = new double[2, 2];
                MathAssistant.Matrix_X_(Q_plus_inv, Q_minus, ref H);

                double dT_unsupport_plus = H[0, 0] * dT_unsupport_minus + H[0, 1] * dT_support_minus;
                double dT_support_plus = H[1, 0] * dT_unsupport_minus + H[1, 1] * dT_support_minus;

                double T_unsupport_plus = T_support_minus;
                double T_support_plus = T_unsupport_minus;

                q_next[0] = T_unsupport_plus;
                q_next[1] = T_support_plus;
                q_next[2] = dT_unsupport_plus;
                q_next[3] = dT_support_plus;
            }
        }


        override public void Integration(MemoryPad pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {

            Qd(q, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k1);

            MathAssistant.vector_vlinear(pad.k1, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k2);

            MathAssistant.vector_vlinear(pad.k2, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k3);

            MathAssistant.vector_vlinear(pad.k3, 1, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k4);

            MathAssistant.vector_vlinear(pad.k1, 1.0 / 6.0, q, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k2, 1.0 / 3.0, pad.Qd_out, ref pad.Qd_in);
            MathAssistant.vector_vlinear(pad.k3, 1.0 / 3.0, pad.Qd_in, ref pad.Qd_out);

            MathAssistant.vector_vlinear(pad.k4, 1.0 / 6.0, pad.Qd_out, ref q_next);

            q_next[5] = q[5];
            q_next[6] = q[6];

            double T_unsupport_minus = q_next[0];
            double T_support_minus = q_next[1];
            double dT_unsupport_minus = q_next[2];
            double dT_support_minus = q_next[3];
            
            double T_alpha = T_unsupport_minus - T_support_minus;
            double shouldBeZero = T_unsupport_minus + T_support_minus - Math.PI - Math.PI - Math.PI - Math.PI + phi + phi;

            double l = a + b;
            double cos_Alpha = Math.Cos(T_alpha);
            double d_Alpha = dT_unsupport_minus - dT_support_minus;
            double cos_support = Math.Cos(T_support_minus);
            double sin_support = Math.Sin(T_support_minus);
            double cos_unsupport = Math.Cos(T_unsupport_minus);
            double sin_unsupport = Math.Sin(T_unsupport_minus);

            double dx_step = -l * sin_support + l * sin_unsupport;
            double dy_step = l * cos_support - l * cos_unsupport;

            double aoa = Math.Atan2(dy_step, dx_step) / Math.PI * 180;

            if (
                //Math.Abs(shouldBeZero) < 0.001 &&
                //T_alpha > min_alpha &&
                T_unsupport_minus > Math.PI * 2 &&
                dT_unsupport_minus < 0 &&
                aoa < 0
                )
            {
                double[,] Q_minus = new double[2, 2];
                Q_minus[0, 0] = -m * a * b;
                Q_minus[0, 1] = -m * a * b + (mH * l * l + 2 * m * a * l) * cos_Alpha;
                Q_minus[1, 0] = 0;
                Q_minus[1, 1] = -m * a * b;

                double[,] Q_plus = new double[2, 2];
                Q_plus[0, 0] = m * b * (b - l * cos_Alpha);
                Q_plus[0, 1] = m * l * (l - b * cos_Alpha) + m * a * a + mH * l * l;
                Q_plus[1, 0] = m * b * b;
                Q_plus[1, 1] = -m * b * l * cos_Alpha;

                double[,] Q_plus_inv = new double[2, 2];
                if (MathAssistant.Matrix_Inverse_2x2(Q_plus, ref Q_plus_inv) == false)
                    throw new Exception("inverse failure");

                double[,] H = new double[2, 2];
                MathAssistant.Matrix_X_(Q_plus_inv, Q_minus, ref H);

                double dT_unsupport_plus = H[0, 0] * dT_unsupport_minus + H[0, 1] * dT_support_minus;
                double dT_support_plus = H[1, 0] * dT_unsupport_minus + H[1, 1] * dT_support_minus;

                double T_unsupport_plus = T_support_minus;
                double T_support_plus = T_unsupport_minus;

                q_next[0] = T_unsupport_plus;
                q_next[1] = T_support_plus;
                q_next[2] = dT_unsupport_plus;
                q_next[3] = dT_support_plus;

                if (q_next[4] >= 0)
                    q_next[4] = -1;
                else
                    q_next[4] = 1;

                

                q_next[5] += dx_step;
                q_next[6] += dy_step;

                //Debugger.Break();
            }
        }

        public bool PropagateAlias(MemoryPad pad, double[] a_start, double[] u, double duration, ref double[] a_end, ref double step_cost)
        {
            int n_step = (int)Math.Floor(duration / t_delta + 0.5);
            if (n_step == 0)
                throw new Exception("integration failure. too small t_delta?");
            step_cost = 0;
            MathAssistant.vector_set(a_start, ref pad.q_tmp);
            for (int i = 0; i < n_step; ++i)
            {
                this.IntegrationAlias(pad, pad.q_tmp, u, t_delta, ref a_end);
                MathAssistant.vector_set(a_end, ref pad.q_tmp);
                step_cost += t_delta;
            }
            return inStateSpace(a_end);
        }

        override public bool Propagate(MemoryPad pad, double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            if (q_end == null)
                q_end = new double[dof_state];

            int n_step = (int)Math.Floor(duration / t_delta + 0.5);

            if (n_step == 0)
                throw new Exception("integration failure. too small t_delta?");

            
            step_cost = 0;

            double[] q_tmp = new double[7];
            translate_AB_to_NS(q_start, ref q_tmp);

                MathAssistant.vector_set(q_tmp, ref pad.q_tmp);
                MathAssistant.vector_set(q_tmp, ref q_end);
                for (int i = 0; i < n_step; ++i)
                {
                    this.Integration(pad, pad.q_tmp, u, t_delta, ref q_end);
                    MathAssistant.vector_set(q_end, ref pad.q_tmp);
                    step_cost += t_delta;
                }

            translate_NS_to_AB(pad.q_tmp, ref q_end);

            return inStateSpace(q_end);
        }

        override public bool inStateSpace(double[] a)
        {
            for (int i = 0; i < dof_alias; ++i)
            {
                if (a[i] < min_alias[i] || a[i] > max_alias[i])
                    return false;
            }
            return true;
        }
    }
}
